/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

#include "dhl_packet.h"
#include "dhl-struct.h"
#include "definedCommands.h"

#ifdef __cplusplus
extern "C" {
#endif /* __cplusplus */

cu_uint_t cupic_to_xb(cu_uint_t var) {
    return (var >> 8) | (var << 8);
}

void build_xbee_header(struct xb_tx_hdr *hdr, cu_uint_t size, cu_uint_t dest) {
    hdr->delimeter = 0x7E;
    hdr->size = cupic_to_xb(size);
    hdr->identifier = 1;                // Data packet w / 16 bit delimeter
    hdr->response = 0;                  // disable the response frame
    hdr->dest = cupic_to_xb(dest);      // the destination
    hdr->disable_ack = 1;               // disable acks
}


void build_dhl_header(struct dhl_hdr *hdr, 
    cu_uint_t size, 
    cu_uint_t src, 
    cu_uint_t dst, 
    cu_uchar_t type,
    cu_uchar_t seq) {

    hdr->src = src;
    hdr->dst = dst;
    hdr->size = cupic_to_xb(size);
    hdr->type = type;
    hdr->seq = seq;
}


cu_uint_t build_led_packet(cu_uchar_t *buf, 
    cu_uint_t buf_size,
    cu_uint_t src, 
    cu_uint_t dest,
    cu_uchar_t seq_num,
    cu_uchar_t cmd) {
    cu_uint_t packet_size;

    // +1 for LED packet size, + 1 for csum
    packet_size = DHL_PAYLOAD + 1 + 1;

    // build the headers
    build_xbee_header((struct xb_tx_hdr*)buf, packet_size - 4, dest);
    build_dhl_header((struct dhl_hdr*)&buf[XB_TX_PAYLOAD],
            1, 
            src, 
            dest, 
            CMD_LEDS, 
            seq_num);

    // the CMD_LEDs payload is just an 8 byte packet where the least significant
    // four bits represent the state
    buf[DHL_PAYLOAD] = cmd;

    buf[packet_size - 1] = build_checksum(buf, CSUM_START, packet_size-1);

    return packet_size;
}


cu_uint_t build_stream_request(cu_uchar_t *buf, 
    cu_uint_t buf_size,
    cu_uint_t subscriber, 
    cu_uint_t dest,
    cu_uchar_t seq_num,
    cu_uchar_t cmd,
    cu_uchar_t start) {

    cu_uint_t packet_size;
    DHL_StreamRequest_t *stream_request = (DHL_StreamRequest_t*)&buf[DHL_PAYLOAD];

    packet_size = DHL_PAYLOAD + sizeof(DHL_StreamRequest_t) + 1;

    build_xbee_header((struct xb_tx_hdr*)buf, packet_size - 4, dest);
    build_dhl_header((struct dhl_hdr*)&buf[XB_TX_PAYLOAD],
            sizeof(DHL_StreamRequest_t), 
            subscriber, 
            dest, 
            CMD_STREAM_REQ, 
            seq_num);

    stream_request->id = cmd;
    stream_request->client = subscriber;
    stream_request->start = start;

    buf[packet_size - 1] = build_checksum(buf, CSUM_START, packet_size-1);

    return packet_size;
}


#ifdef CUPIC
cu_uint_t build_dhl_capabilities(cu_uchar_t *buf, cu_uint_t src, cu_uchar_t seq_num) {
    cu_uchar_t i;
    cu_uint_t packet_size;
    DHL_Capabilities_t *capabilities;
    
    capabilities = (DHL_Capabilities_t*)&buf[DHL_PAYLOAD];

    // DHL Payload is size of XB header + DHL header
    // +1 for the Checksum
    packet_size = DHL_PAYLOAD + CAPABILITY_SIZE + 1; // 31

    // Setup the headers (don't include first 3 bytes & csum in the xb size)
    build_xbee_header((struct xb_tx_hdr*)buf, packet_size - 4, COORDINATOR_ADDR);
    build_dhl_header((struct dhl_hdr*)&buf[XB_TX_PAYLOAD],
            CAPABILITY_SIZE, 
            src, 
            COORDINATOR_ADDR, 
            CMD_CAPABILITY, 
            seq_num);

    // Set up capabilities packet data
    capabilities->id = (cu_uchar_t)src;

    // FIXME: When the CUPIC's got more capability subsystems, we'll need to 
    //        let the user configure this option
    capabilities->sub_system_id = CMD_CUPIC;

    // fill out what streams we have
    capabilities->streams = 0;
    for (i = 0; i < NUM_STREAMS; i++)
        capabilities->streams |= (0x1 << ((ds_streams[i] & 0x0F) - 1));

    // fill out what commands we have
    capabilities->commands = 0;
    for (i = 0; i < NUM_COMMANDS; i++)
        capabilities->commands |= (0x1 << ((ds_commands[i] & 0x0F)- 1));

    // Set the attributes to zero for now
    for (i = 0; i < CAP_ATTRIBUTES; i++)
        capabilities->attributes[i] = 0;

    // XB Checksum (note that it ignores the first 3 bytes & dont include the checksum)
    buf[packet_size-1] = build_checksum(buf, CSUM_START, packet_size-1);

    return packet_size;
}


void dhl_message_recieved() {
    struct xb_rx_hdr *rx_hdr;
    struct dhl_hdr* dhl;
    unsigned char i, column;

    // Checksum is valid because XBee throws out packets with invalid csums
    // extract pointers to the rx_hdr and the dhl_header
    rx_hdr = (struct xb_rx_hdr*)USART_inXB;
    rx_hdr->dest = cupic_to_xb(rx_hdr->dest);

    dhl = (struct dhl_hdr*)&USART_inXB[XB_RX_PAYLOAD];

    // set the incoming RSSI power
    incoming_RSSI = rx_hdr->rssi_power;

    // Let someone else handle this packet type
    if (rx_hdr->size == 3) return;

    switch (dhl->type) {
        case CMD_STREAM_REQ: {
            DHL_StreamRequest_t *request = (DHL_StreamRequest_t*)&USART_inXB[DHL_PAYLOAD];

            // figure out the capability type
            for (i = 0; i < NUM_STREAMS; i++) {
                if (request->id == ds_streams[i])
                    break;
            }
            column = i;

            // someone requested a capability type we dont have
            if (column == NUM_STREAMS)
                break;

            // we know the capability's column
            if ( request->start ) { // 
                // insert the subscribers address into the approriate location
                for (i = 0; i < MAX_FLOCK_SIZE; i++) {
                    if (subscription_addresses[i][column] == 0) {
                        subscription_addresses[i][column] = rx_hdr->dest;
                        break;
                    }
                }
            } else {
                // remove the subscriber's subscription by zeroing thier
                // address in the subscription_addressess table
                for (i = 0; i < MAX_FLOCK_SIZE; i++) {
                    if (subscription_addresses[i][column] == rx_hdr->dest) {
                        subscription_addresses[i][column] = 0;
                        break;
                    }
                }
            }

            // FIXME: I'm not sure if stream request have acks or if 
            //   recieving the data stream is a sufficient acknowledgement
        } break;

        case CMD_LEDS: {
            DHL_LED_t* dhl_leds = (DHL_LED_t*)&USART_inXB[DHL_PAYLOAD];
            cu_uchar_t packet_size = XB_TX_PAYLOAD + DHL_PAYLOAD + 2;

            // Set the LEDS
            if (CUPIC_LED0_MASK & dhl_leds->LEDS)
                LED1 = 1;
            else
                LED1 = 0;

            if (CUPIC_LED1_MASK & dhl_leds->LEDS)
                LED2 = 1;
            else
                LED2 = 0;

            if (CUPIC_LED2_MASK & dhl_leds->LEDS)
                LED3 = 1;
            else
                LED3 = 0;

            if (CUPIC_LED3_MASK & dhl_leds->LEDS)
                LED4 = 1;
            else
                LED4 = 0;

            // Build the confirmation packet
            build_xbee_header((struct xb_tx_hdr*)USART_outXB, packet_size - 4, rx_hdr->dest);
            build_dhl_header((struct dhl_hdr*)&USART_outXB[XB_TX_PAYLOAD],
                    dhl->size,
                    NODE_ID,
                    rx_hdr->dest,
                    dhl->type,
                    dhl->seq+1);

            USART_outXB[DHL_PAYLOAD] = dhl_leds->LEDS;
            USART_outXB[packet_size - 1] = build_checksum(USART_outXB, CSUM_START, packet_size - 1);

            // Send the packet (packet size should be a little bigger)
            XB_send(USART_outXB, packet_size - 1);

            break;
        }
        default:
            break;
    }
}


void stream_data(void) {
    unsigned char i, j;
    cu_uint_t addr = 0;

    // only send data streams to nodes who have subscribed
    // if an address exists in the subscription_addressess table, we send
    // a packet (with a type that correspond's to the address's column) 
    // to that address

    // Note that 0000 is NOT a valid address

    for (i = 0; i < NUM_ROWS; i++) {
        for (j = 0; j < NUM_COLUMNS; j++) {
            addr = subscription_addresses[i][j];

            if (addr != 0) {
                cu_uchar_t bytes_to_send; 

                // build the packet
                // FIXME: as more capabilities are added, add the function blocks here
                switch (ds_streams[j]) {
                    case CMD_DEBUG:
                        bytes_to_send = build_debug_packet(USART_outXB, 128, addr, packet_number);
                        break;
                    case CMD_HOUSE_KEEPING:
                        bytes_to_send = build_house_keeping_packet(USART_outXB, 128, addr);
                        break;
                    default:
                        break;
                }

                // FIXME: XB_send should want the packet length, not packet length -1
                XB_send(USART_outXB, bytes_to_send - 1);
            }
        }
    }
}


cu_uint_t build_dhl_custody(cu_uchar_t *buf, cu_uint_t src, cu_uint_t dest, cu_uchar_t seq_num) {
    DHL_Custody_t* custody;
    cu_uint_t packet_size;

    packet_size = DHL_PAYLOAD + sizeof(DHL_Custody_t) + 1;

    // build the header
    build_xbee_header((struct xb_tx_hdr*)buf, packet_size - 4, dest);
    build_dhl_header((struct dhl_hdr*)&buf[XB_TX_PAYLOAD],
            sizeof(DHL_Custody_t), 
            src, 
            dest, 
            CMD_CUSTODY, 
            seq_num);

    custody = (DHL_Custody_t*)&buf[DHL_PAYLOAD];

    // build the payload
    custody->parent = COORDINATOR_ADDR;
    custody->requesting = 0;

    buf[packet_size - 1] = build_checksum(buf, CSUM_START, packet_size-1);

    return packet_size;
}


cu_uint_t build_debug_packet(cu_uchar_t *buf, cu_uint_t max_size, cu_uint_t dest, cu_uint_t seq_num) {
    cu_uint_t packet_size;
    DHL_DebugPacket_t *debug = (DHL_DebugPacket_t*)&buf[DHL_PAYLOAD];

    packet_size = DHL_PAYLOAD + sizeof(DHL_DebugPacket_t) + 1;

    // build the headers
    build_xbee_header((struct xb_tx_hdr*)buf, packet_size - 4, dest);
    build_dhl_header((struct dhl_hdr*)&buf[XB_TX_PAYLOAD],
            sizeof(DHL_DebugPacket_t), 
            NODE_ID, 
            dest, 
            CMD_DEBUG,
            seq_num);

    // minor cmd_alt calculations 
    // (might need to go imeediately before cmd_alt is set
    if (cmd_alt < -10000)
        cmd_alt = -10000;
    else if (cmd_alt > 10000)
        cmd_alt = 10000;

    // pack the struct
    debug->packet_number    = packet_number;
    debug->heading          = heading;
    debug->lon              = lon_pos;
    debug->lat              = lat_pos;
    debug->palt_msw         = palt_telem >> 8;
    debug->palt_lsb         = (cu_uchar_t)palt_telem;
    debug->roll_est_g       = roll_est_g_i;
    debug->flight_status    = flight_status;
    debug->helix_top_alt    = helix_top_alt;
    debug->helix_btm_alt    = helix_btm_alt;
    debug->cmd_alt          = cmd_alt;
    debug->pitot_as         = pitot_as * 1000;
    debug->gy_temp          = gy_temp;
    debug->throttle         = throttle;
    debug->alt              = alt;
    debug->speed            = speed;

    buf[packet_size - 1] = build_checksum(buf, CSUM_START, packet_size-1);

    return packet_size;
}


cu_uint_t build_data_packet(cu_uchar_t *buf, cu_uint_t max_size, cu_uint_t dest, cu_uint_t seq_num) {
    cu_uint_t packet_size;
    unsigned char i;
    DHL_DataPacket_t *data_packet = (DHL_DataPacket_t*)&buf[DHL_PAYLOAD];

    packet_size = DHL_PAYLOAD + sizeof(DHL_DataPacket_t) + 1;

    // build the header
    build_xbee_header((struct xb_tx_hdr*)buf, packet_size - 4, dest);
    build_dhl_header((struct dhl_hdr*)&buf[XB_TX_PAYLOAD],
            sizeof(DHL_DataPacket_t), 
            NODE_ID, 
            dest, 
            CMD_CUPIC_DATA,
            seq_num);

    // Pack the struct
    data_packet->packet_number = data_packet_number;
    data_packet->palt_msw = palt_telem>>8;
    data_packet->palt_lsb = palt_telem;
    data_packet->tow = tow;
    
    // Get data from SPI in pkt
    for (i = 0; i < 24; i++)  // from SPI packet size
        data_packet->spi[i] = SPI_in_pkt[i];

    data_packet->pitot_as = pitot_as * 1000;

    buf[packet_size - 1] = build_checksum(buf, CSUM_START, packet_size-1);

    return packet_size;
}

cu_uint_t build_house_keeping_packet(cu_uchar_t *buf, cu_uint_t max_size, cu_uint_t dest) {
    cu_uint_t packet_size;
    DHL_HouseKeeping_t *house_keeping = (DHL_HouseKeeping_t*)&buf[DHL_PAYLOAD];

    packet_size = DHL_PAYLOAD + sizeof(DHL_HouseKeeping_t) + 1;

    build_xbee_header((struct xb_tx_hdr*)buf, packet_size - 4, dest);
    build_dhl_header((struct dhl_hdr*)&buf[XB_TX_PAYLOAD],
            sizeof(DHL_HouseKeeping_t), 
            NODE_ID, 
            dest, 
            CMD_HOUSE_KEEPING,
            hkpacket_number);

    // Pack the struct
    house_keeping->packet_number = hkpacket_number;
    house_keeping->battery_voltage = cal_battery_voltage * 100.;
    house_keeping->x_loiter_center = x_loiter_center_coords;
    house_keeping->y_loiter_center = y_loiter_center_coords;
    house_keeping->r_d = r_d;
    house_keeping->ascent_rate = ascent_rate;
    house_keeping->descent_rate = descent_rate;
    house_keeping->r_bound = r_bound;
    house_keeping->bound_alt = bound_alt;
    house_keeping->x_bound_center = x_bound_center_coords;
    house_keeping->y_bound_center = y_bound_center_coords;
    house_keeping->version_num = version_num;
    house_keeping->flight_term_status = flight_term_status;
    house_keeping->incoming_RSSI = incoming_RSSI;
    house_keeping->time_left = time_left;

    buf[packet_size - 1] = build_checksum(buf, CSUM_START, packet_size-1);

    return packet_size;
}

cu_uint_t build_high_speed_packet(cu_uchar_t *buf, cu_uint_t max_size, cu_uint_t dest) {
    cu_uint_t packet_size;
    DHL_HighSpeed_t *high_speed = (DHL_HighSpeed_t*)&buf[DHL_PAYLOAD];

    packet_size = DHL_PAYLOAD + sizeof(DHL_HighSpeed_t) + 1;

    build_xbee_header((struct xb_tx_hdr*)buf, packet_size - 4, dest);
    build_dhl_header((struct dhl_hdr*)&buf[XB_TX_PAYLOAD],
            sizeof(DHL_HighSpeed_t), 
            NODE_ID, 
            dest, 
            CMD_HIGH_SPEED,
            hspacket_number);

    if (throttle > -650)
        motor_on = 1;
    else
        motor_on = 0;

    // fill the high_speed packet
    for (i = 0; i < 10; i++)
        high_speed->data[i] = hs_data1[i];
    for (i = 0; i < 10; i++)
        high_speed->data[i+10] = hs_data2[i];

    high_speed->tow = hspacket_number;
    high_speed->packet_number = hspacket_number++;
    high_speed->motor_on = motor_on;
    
    buf[packet_size - 1] = build_checksum(buf, CSUM_START, packet_size-1);

    return packet_size;
}

#endif //CUPIC


cu_uchar_t build_checksum(cu_uchar_t *buf, int start, int end) {
    cu_uchar_t sum = 0;
    int a;

    for (a = start; a < end; a++)
        sum = sum + buf[a];

    return (255 - sum);
}


#ifdef __cplusplus
};
#endif /* __cplusplus */
